#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <string>
#include <vector>
#include <iostream>
#include "serial.hpp"
#include "sensor.hpp"
#include "json_data.hpp"
class SensorNode : public rclcpp::Node
{
public:
    SensorNode() : Node("SensorNode")
    {
        declare_all_parameters();
        get_all_parameters();
        sensor_drv=new Sensor_Drv(serial_port0,baud_rate0_);
        sensor_drv->motor_id=motor_id;
        sensor_drv->zhuagou_id=zhuagou_id;
        sensor_drv->sensor0_id =sensor0_id;
        sensor_drv->sensor1_id =sensor1_id;
        sensor_drv->sensor2_id =sensor2_id;
        sensor_drv->sensor3_id =sensor3_id;
        sensor_drv->sensor4_id =sensor4_id;
        sensor_drv->sensor5_id =sensor5_id;
        // 创建ROS发布者
        ros_sensor_pub_ = this->create_publisher<std_msgs::msg::String>(
            ros_sensor_pub_name,
            10
        );
        // 创建ROS订阅者（接收ROS消息并转发到motor）
        ros_motor_downup_sub_ = this->create_subscription<std_msgs::msg::String>(
            ros_motor_downup_sub_name,
            10,
            std::bind(&SensorNode::ros_message_callback, this, std::placeholders::_1)
        );
        RCLCPP_INFO(this->get_logger(),"pub topic=%s",ros_motor_downup_sub_name.c_str());
        timer_ = this->create_wall_timer(std::chrono::milliseconds(30),
            std::bind(&SensorNode::time_publish_message, this));
        //RCLCPP_INFO(this->get_logger(),"serial no= %s motor =%x sensor= %x",serial_port0.c_str(),motor_id,sensor0_id);
    }
    int run_mode;
    int baud_rate0_;
    std::string serial_port0;
    Sensor_Drv *sensor_drv;
    int sensor0_id,sensor1_id;
    int sensor2_id,sensor3_id,sensor4_id,sensor5_id;
    int motor_id,zhuagou_id;
    void get_all_parameters(void);
    void declare_all_parameters(void);
    std::string ros_sensor_pub_name;
    std::string ros_motor_downup_sub_name;
    JsonData_sensor_convert json_msg;
    rclcpp::TimerBase::SharedPtr timer_;
    void time_publish_message(void);
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_motor_downup_sub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ros_sensor_pub_;
    void ros_message_callback(const std_msgs::msg::String::SharedPtr msg);
}; 
 void SensorNode::time_publish_message(void)
 {
    json_msg.motor_cur_speed = sensor_drv->motor_cur_speed;
    json_msg.distance_up = sensor_drv->distance1;
    json_msg.distance_down = sensor_drv->distance0;
    json_msg.dis_F = sensor_drv->distance2;
    json_msg.dis_B = sensor_drv->distance3;
    json_msg.dis_L = sensor_drv->distance4;
    json_msg.dis_R = sensor_drv->distance5;
    json_msg.set_json_msg();
    auto msg = std_msgs::msg::String();
    msg.data = json_msg.json_msg;
    ros_sensor_pub_->publish(msg);
    /////
    RCLCPP_INFO(this->get_logger(),"DWdis0=%5.2f UPdis1=%5.2f",sensor_drv->distance0,sensor_drv->distance1);
    RCLCPP_INFO(this->get_logger(),"Fdis2=%5.2f Bdis3=%5.2f Ldis4=%5.2f Rdis5=%5.2f ",
        sensor_drv->distance2,sensor_drv->distance3,sensor_drv->distance4,sensor_drv->distance5);
 }   
void SensorNode::ros_message_callback(const std_msgs::msg::String::SharedPtr msg)
{
    //json_data.json_msg=msg->data;
    json_msg.receive_msg(msg->data);
    sensor_drv->motor_set_speed=0;
    sensor_drv->zhuagou_cmd=0;
    if(json_msg.key_value==0)
     sensor_drv->motor_set_speed = 0;
    if(json_msg.key_value==4)
     sensor_drv->motor_set_speed = 20000; 
    if(json_msg.key_value==8)
     sensor_drv->motor_set_speed = -20000; 
    if(json_msg.key_value==1) 
     sensor_drv->zhuagou_cmd=1;
    if(json_msg.key_value==2) 
     sensor_drv->zhuagou_cmd=2; 
    RCLCPP_INFO(this->get_logger(),"receive speed set= %d",sensor_drv->motor_set_speed);
    // std::string ssg=msg->data;
    // RCLCPP_INFO(this->get_logger(),"receive msg=%s",ssg.c_str());
}
void SensorNode::get_all_parameters(void)
{
    this->get_parameter("run_mode", run_mode); //0=正常运行模式 1=查看接收到的消息数量 2=查看接收到的消息内容
    this->get_parameter("serial_port0", serial_port0);
    this->get_parameter("baud_rate0", baud_rate0_);
    this->get_parameter("sensor0_id", sensor0_id);
    this->get_parameter("sensor1_id", sensor1_id);
    this->get_parameter("sensor2_id", sensor2_id);
    this->get_parameter("sensor3_id", sensor3_id);
    this->get_parameter("sensor4_id", sensor4_id);
    this->get_parameter("sensor5_id", sensor5_id);
    this->get_parameter("motor_id", motor_id);
    this->get_parameter("zhuagou_id", zhuagou_id);
    this->get_parameter("ros_sensor_pub_name",ros_sensor_pub_name);
    this->get_parameter("ros_motor_downup_sub_name",ros_motor_downup_sub_name);
    
}
void SensorNode::declare_all_parameters(void)
{
    this->declare_parameter<int>("run_mode", 0);
    this->declare_parameter<std::string>("serial_port0", "/dev/ttyUSB0");
    this->declare_parameter<int>("baud_rate0", 115200);
    this->declare_parameter<int>("sensor0_id", 0x181);
    this->declare_parameter<int>("sensor1_id", 0x182);
    this->declare_parameter<int>("sensor2_id", 0x183);
    this->declare_parameter<int>("sensor3_id", 0x184);
    this->declare_parameter<int>("sensor4_id", 0x185);
    this->declare_parameter<int>("sensor5_id", 0x186);
    this->declare_parameter<int>("motor_id", 0x174);
    this->declare_parameter<int>("zhuagou_id", 0x174);
    
    this->declare_parameter("ros_sensor_pub_name","ros_sensor_pub_name");
    this->declare_parameter("ros_motor_downup_sub_name","ros_motor_downup_sub_name");
    //zhuagou_id
}
int main(int argc, char * argv[])
{  
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SensorNode>());
    rclcpp::shutdown();
    return 0;
}